/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package search;

import java.util.ArrayList;
import java.util.List;
import java.util.Stack;
import plot.GNUPlot;
import world.Obstacle;
import world.Point;
import world.World;

/**
 *
 * @author dheath
 */
public class Searcher
{

    private Search search;
    private List<WorldNode> nodes;
    private int nodeSize;

    public Searcher(Search search)
    {
        this.search = search;
        this.nodeSize = 10;
    }

    public Stack<Point> search(Point s, Point e, World world, GNUPlot plot)
    {
        if (nodes == null || nodes.size() < 1)
        {
            buildSearchGraph(world);
        }

        WorldNode start = getNode(s);
        WorldNode end = getNode(e);

        if (start != null && end != null)
        {

            PriorityNode endNode = search.searchWorld(start, end, plot);


            return buildStack(endNode);
        } else
        {
            System.out.println("POINTS DO NOT EXIST IN WORLD.");
        }
        return null;
    }

    private Stack<Point> buildStack(PriorityNode end)
    {
        Stack<Point> path = new Stack();

        PriorityNode current = end;
        double slope = 0.0;
        double b = 500.0;

        while (current != null)
        {
            if (current.getParent() == null)
            {
                path.push(current.getNode().getCenter());
            } else
            {
                if (!onLine(current.getParent().getNode().getCenter(), slope, b))
                {
                    path.push(current.getNode().getCenter());

                    slope = (current.getParent().getNode().getCenter().getY() - current.getNode().getCenter().getY()) /
                            (current.getParent().getNode().getCenter().getX() - current.getNode().getCenter().getX());

                    b = current.getNode().getCenter().getY() - (slope * current.getNode().getCenter().getX());

                    if (slope == Double.POSITIVE_INFINITY || slope == Double.NEGATIVE_INFINITY)
                    {
                        b = current.getParent().getNode().getCenter().getX();
                    }
                }
            }
            current = current.getParent();
        }

        return path;
    }

    private boolean onLine(Point p, double slope, double b)
    {
        if (slope == Double.POSITIVE_INFINITY || slope == Double.NEGATIVE_INFINITY)
        {
            double result = Math.abs(p.getX() - b);

            return (result < .05);
        }
        double result = p.getX() * slope + b;
        result = Math.abs(result - p.getY());

        return (result < .05);
    }

    public void buildSearchGraph(World world)
    {
        //System.out.println("Building graph");

        double worldsize = Double.parseDouble(world.getConstants().get("worldsize"));
        int row = (int) (worldsize / nodeSize);

        nodes = new ArrayList();
        for (double y = (worldsize / 2.0); y > (-1 * worldsize / 2.0); y -= nodeSize)
        {
            for (double x = (-1.0 * worldsize / 2.0); x < (worldsize / 2.0); x += nodeSize)
            {

                WorldNode node = new WorldNode(new Point(x, y), nodeSize);

                nodes.add(node);
            }
        }

        for (int i = 0; i < nodes.size(); i++)
        {
            WorldNode curNode = nodes.get(i);

            if (i < row)
            {
                if (i % row == 0)
                {
                    curNode.setEast(nodes.get(i + 1));
                    curNode.setSouthEast(nodes.get(i + row + 1));
                } else
                {
                    if (i % row == row - 1)
                    {
                        curNode.setWest(nodes.get(i - 1));
                        curNode.setSouthWest(nodes.get(i + row - 1));
                    } else
                    {
                        curNode.setEast(nodes.get(i + 1));
                        curNode.setSouthEast(nodes.get(i + row + 1));
                        curNode.setWest(nodes.get(i - 1));
                        curNode.setSouthWest(nodes.get(i + row - 1));
                    }
                }
                curNode.setSouth(nodes.get(i + row));
            } else
            {
                if (i >= nodes.size() - row)
                {
                    if (i % row == 0)
                    {
                        curNode.setNorthEast(nodes.get(i - row + 1));
                        curNode.setEast(nodes.get(i + 1));
                    } else
                    {
                        if (i % row == row - 1)
                        {
                            curNode.setNorthWest(nodes.get(i - row - 1));
                            curNode.setWest(nodes.get(i - 1));
                        } else
                        {
                            curNode.setNorthEast(nodes.get(i - row + 1));
                            curNode.setEast(nodes.get(i + 1));
                            curNode.setNorthWest(nodes.get(i - row - 1));
                            curNode.setWest(nodes.get(i - 1));
                        }
                    }
                    curNode.setNorth(nodes.get(i - row));
                } else
                {
                    if (i % row == 0)
                    {
                        curNode.setNorthEast(nodes.get(i - row + 1));
                        curNode.setEast(nodes.get(i + 1));
                        curNode.setSouthEast(nodes.get(i + row + 1));
                    } else
                    {
                        if (i % row == row - 1)
                        {
                            curNode.setWest(nodes.get(i - 1));
                            curNode.setSouthWest(nodes.get(i + row - 1));
                            curNode.setNorthWest(nodes.get(i - row - 1));
                        } else
                        {
                            curNode.setWest(nodes.get(i - 1));
                            curNode.setSouthWest(nodes.get(i + row - 1));
                            curNode.setNorthWest(nodes.get(i - row - 1));
                            curNode.setNorthEast(nodes.get(i - row + 1));
                            curNode.setEast(nodes.get(i + 1));
                            curNode.setSouthEast(nodes.get(i + row + 1));
                        }
                    }
                    curNode.setNorth(nodes.get(i - row));
                    curNode.setSouth(nodes.get(i + row));
                }
            }

            curNode.setCost(getCost(curNode, world));
        }
    }

    private WorldNode getNode(Point p)
    {
        for (WorldNode node : nodes)
        {
            if (p.getX() >= node.getTopLeft().getX() && p.getY() <= node.getTopLeft().getY() &&
                    p.getX() < node.getTopLeft().getX() + nodeSize && p.getY() > node.getTopLeft().getY() - nodeSize)
            {
                return node;
            }
        }
        return null;
    }

    private boolean insideObstacle(Obstacle ob, Point p)
    {
        boolean c = false;

        if ((((ob.getTLPoint().getY() <= p.getY()) && (p.getY() < ob.getBLPoint().getY())) ||
                ((ob.getBLPoint().getY() <= p.getY()) && (p.getY() < ob.getTLPoint().getY()))) &&
                (p.getX() < (ob.getBLPoint().getX() - ob.getTLPoint().getX()) * (p.getY() - ob.getTLPoint().getY()) / (ob.getBLPoint().getY() - ob.getTLPoint().getY()) + ob.getTLPoint().getX()))
        {
            c = !c;
        }
        if ((((ob.getTRPoint().getY() <= p.getY()) && (p.getY() < ob.getTLPoint().getY())) ||
                ((ob.getTLPoint().getY() <= p.getY()) && (p.getY() < ob.getTRPoint().getY()))) &&
                (p.getX() < (ob.getTLPoint().getX() - ob.getTRPoint().getX()) * (p.getY() - ob.getTRPoint().getY()) / (ob.getTLPoint().getY() - ob.getTRPoint().getY()) + ob.getTRPoint().getX()))
        {
            c = !c;
        }
        if ((((ob.getBRPoint().getY() <= p.getY()) && (p.getY() < ob.getTRPoint().getY())) ||
                ((ob.getTRPoint().getY() <= p.getY()) && (p.getY() < ob.getBRPoint().getY()))) &&
                (p.getX() < (ob.getTRPoint().getX() - ob.getBRPoint().getX()) * (p.getY() - ob.getBRPoint().getY()) / (ob.getTRPoint().getY() - ob.getBRPoint().getY()) + ob.getBRPoint().getX()))
        {
            c = !c;
        }
        if ((((ob.getBLPoint().getY() <= p.getY()) && (p.getY() < ob.getBRPoint().getY())) ||
                ((ob.getBRPoint().getY() <= p.getY()) && (p.getY() < ob.getBLPoint().getY()))) &&
                (p.getX() < (ob.getBRPoint().getX() - ob.getBLPoint().getX()) * (p.getY() - ob.getBLPoint().getY()) / (ob.getBRPoint().getY() - ob.getBLPoint().getY()) + ob.getBLPoint().getX()))
        {
            c = !c;
        }

        return c;
    }

    private double getCost(WorldNode node, World world)
    {
        int count = 0;
        double avgDist = 0.0;
        double worldsize = Double.parseDouble(world.getConstants().get("worldsize"));

        for (Obstacle ob : world.getObstacles())
        {
            if (insideObstacle(ob, node.getCenter()))
            {
                return Double.POSITIVE_INFINITY;
            }

            double dist = Math.sqrt(Math.pow(ob.getTLPoint().getX() - node.getCenter().getX(), 2) +
                    Math.pow(ob.getTLPoint().getY() - node.getCenter().getY(), 2));
            if (dist < nodeSize * 1)
            {
                count++;
                avgDist += dist;
            }
            dist = Math.sqrt(Math.pow(ob.getTRPoint().getX() - node.getCenter().getX(), 2) +
                    Math.pow(ob.getTRPoint().getY() - node.getCenter().getY(), 2));
            if (dist < nodeSize * 1)
            {
                count++;
                avgDist += dist;
            }
            dist = Math.sqrt(Math.pow(ob.getBRPoint().getX() - node.getCenter().getX(), 2) +
                    Math.pow(ob.getBRPoint().getY() - node.getCenter().getY(), 2));
            if (dist < nodeSize * 1)
            {
                count++;
                avgDist += dist;
            }
            dist = Math.sqrt(Math.pow(ob.getBLPoint().getX() - node.getCenter().getX(), 2) +
                    Math.pow(ob.getBLPoint().getY() - node.getCenter().getY(), 2));
            if (dist < nodeSize * 1)
            {
                count++;
                avgDist += dist;
            }

        }

        double dist = Math.sqrt(Math.pow(worldsize / 2 - node.getCenter().getX(), 2));
        if (dist < nodeSize * 2)
        {
            count++;
            avgDist += dist;
        }
        dist = Math.sqrt(Math.pow(-1 * worldsize / 2 - node.getCenter().getX(), 2));
        if (dist < nodeSize * 2)
        {
            count++;
            avgDist += dist;
        }
        dist = Math.sqrt(Math.pow(-1 * worldsize / 2 - node.getCenter().getY(), 2));
        if (dist < nodeSize * 2)
        {
            count++;
            avgDist += dist;
        }
        dist = Math.sqrt(Math.pow(worldsize / 2 - node.getCenter().getY(), 2));
        if (dist < nodeSize * 2)
        {
            count++;
            avgDist += dist;
        }

        avgDist = avgDist / count;
        double weight = (worldsize / 4 * (5 / avgDist));
        if (Double.isNaN(weight))
        {
            weight = 0;
        }

        return node.getLength() + weight;

    }
}
